/*
 * final_movement.h
 *
 * Created: 12/4/2013 2:45:43 PM
 *  Author: Andrew Gulstad: all the code, Daniel Keith:editing 
 */ 
#include "open_interface.h"
#include "lcd.h"
#include "util.h"
#include <stdio.h>
#include <string.h>
#include "BAM.h"
int Final_movement();//prototype
extern oi_t *sensor_data;

	
void forward(int travel){

	int sum = 0;
	int speed = 150;
	oi_set_wheels(speed, speed); // move forward; full speed
	while (sum < travel/3) {		//robot travels until it reaches input max dist	  //divides by 3 for robot #8
		if (Final_movement() != 1)  //if no obstacles, continues fwd
			{
				oi_update(sensor_data);
				sum += sensor_data->distance;
				if (speed < 210)  	//speed doubles after first cycle
				{
					speed = speed + 50;
				}
			
			}
		else
		{
			break;
		}		
	}
	serial_puts("movement complete\r\n");
	oi_set_wheels(0, 0); // stop
	oi_free(sensor_data);
}
void right_turn(int degree){			

	int16_t angle = 0;
	
	oi_set_wheels(-100, 100); // rotate right
	while (abs(angle) < (degree-6)) {  //angle is decreasing so abs is necessary
		wait_ms(15);
		oi_update(sensor_data);
		angle += sensor_data->angle;
		
		
	}
	oi_set_wheels(0, 0); // stop
	oi_free(sensor_data);
}
void left_turn(int degree){

	int16_t angle = 0;
	
	oi_set_wheels(100, -100); // rotate left
	while (angle < (degree-4)) {
		wait_ms(15);
		oi_update(sensor_data);
		angle += sensor_data->angle;
		
		//lprintf("%d", angle); //for testing the angle of the robot
	}
	oi_set_wheels(0, 0); // stop
	oi_free(sensor_data);
}
void backward(int travel){
	

	int sum = 0;
	oi_set_wheels(-200, -200); // move backward; full speed
	while (abs(sum) < travel/2) {					//divides by 2 for robot #8

		oi_update(sensor_data);
		sum += sensor_data->distance;
	}	
	oi_set_wheels(0, 0); // stop
	oi_free(sensor_data);
}
int Final_movement(void) {
	lcd_init();

	
	wait_ms(15);
	oi_update(sensor_data);
	
	//sets the variables for each cliff sensor equal to the sensor values
	
	uint16_t cliff_frontleft_signal = sensor_data->cliff_frontleft_signal;		
	uint16_t cliff_frontright_signal = sensor_data->cliff_frontright_signal;
	uint16_t cliff_left_signal = sensor_data->cliff_left_signal;
	uint16_t cliff_right_signal = sensor_data->cliff_right_signal;
	 
	//left bumper	
	if (sensor_data->bumper_left)
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal
		
		serial_puts("boulder_detected_on_left\r\n");
		
		return 1;
	}
	
	//right bumper
	
	else if (sensor_data->bumper_right)
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal
		
		serial_puts("boulder_detected_on_right\r\n");
		
		return 1;
	}
	
	//values for cliff sensor triggers were determined based on readings in the 
	//test program cliff.c
	
	// front_left cliff sensor
	
	else if (( cliff_frontleft_signal<50) || (cliff_frontleft_signal>300))
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal										//Robot 8				
		if ( cliff_frontleft_signal<50)												//Frontleft:  low: 150  high: 800
		{																			//Frontright: low: 150  high: 700
			serial_puts("cliff_detected front left\r\n");							//left:       low: 150  high: 800
		}																			//right:      low: 150  high: 800
		else if (cliff_frontleft_signal>300)
		{
			serial_puts("line_detected front left\r\n");							//Robot 16
		}																			//Frontleft:  low: 50   high: 300
																					//Frontright: low: 150   high: 600
		return 1;																	//left:       low: 150	high: 600
	}																				//right:      low: 150  high: 800
	
	//front_right cliff sensor
	
	else if (( cliff_frontright_signal<150) || (cliff_frontright_signal>600))
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal
		if ( cliff_frontright_signal<150)
		{
			serial_puts("cliff_detected front right\r\n");
		}
		else if (cliff_frontright_signal>600)
		{
			serial_puts("line_detected front right\r\n");
		}
		
		return 1;
	}	
	
	//left cliff sensor
	
	else if (( cliff_left_signal<150) || (cliff_left_signal>600))
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal
		if ( cliff_left_signal<150)
		{
			serial_puts("cliff_detected on left\r\n");
		}
		else if (cliff_left_signal>600)
		{
			serial_puts("line_detected on left\r\n");
		}
		
		return 1;
	}	
	
	//right cliff sensor
	
	else if (( cliff_right_signal<150) || (cliff_right_signal>800))
	{
		oi_set_wheels(0, 0); //stop
		backward(30);
		
		//prints out a warning to the terminal
		if ( cliff_right_signal<150)
		{
			serial_puts("cliff_detected on right\r\n");
		}
		else if (cliff_right_signal>800)
		{
			serial_puts("line_detected on right\r\n");
		}
		
		return 1;	
	}
	return 0;
}